#include <stdio.h>
#include <assert.h>
#include <math.h>
#include <stdlib.h>

#include <algorithm>

#include "tetra.h"

//#define DEBUG_VOLVOL 1

using namespace PBVP;

const int Tetra::vert_neighboring[15][15] = {
	{7, 1, 3, 4, 8, 10, 12, 14},
	{7, 0, 2, 5, 9, 10, 12, 14},
	{7, 1, 3, 6, 9, 10, 13, 14},
	{7, 0, 2, 7, 8, 10, 13, 14},
	{7, 0, 5, 7, 8, 11, 12, 14},
	{7, 1, 4, 6, 9, 11, 12, 14},
	{7, 2, 5, 7, 9, 11, 13, 14},
	{7, 3, 4, 6, 8, 11, 13, 14},
	{5, 0, 3, 4, 7, 14},
	{5, 1, 2, 5, 6, 14},
	{5, 0, 1, 2, 3, 14},
	{5, 4, 5, 6, 7, 14},
	{5, 0, 1, 4, 5, 14},
	{5, 2, 3, 6, 7, 14},
	{14, 0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13}
};

// six simplex decomposition of a regular cell
const int Tetra::six_simplex_neighbors[14][3] = {
	{0, 0, 1}, {0, 1, 0}, {1, 0, 0},
	{0, 0, -1}, {0, -1, 0}, {-1, 0, 0},
	{0, 1, 1}, {1, 0, 1}, {1, 1, 0},
	{0, -1, -1}, {-1, 0, -1}, {-1, -1, 0},
	{1, 1, 1}, {-1, -1, -1}
};

const int Tetra::six_tetra_index[6][4][3] = {
	{{0, 0, 0}, {1, 0, 0}, {1, 0, 1}, {1, 1, 1}},
	{{0, 0, 0}, {1, 0, 0}, {1, 1, 0}, {1, 1, 1}},
	{{0, 0, 0}, {0, 1, 0}, {1, 1, 0}, {1, 1, 1}},
	{{0, 0, 0}, {0, 1, 0}, {0, 1, 1}, {1, 1, 1}},
	{{0, 0, 0}, {0, 0, 1}, {1, 0, 1}, {1, 1, 1}},
	{{0, 0, 0}, {0, 0, 1}, {0, 1, 1}, {1, 1, 1}}
};

const int Tetra::six_tetra_verts[6][4] = {
	{0, 1, 5, 7},
	{0, 1, 3, 7},
	{0, 2, 3, 7},
	{0, 2, 6, 7},
	{0, 4, 5, 7},
	{0, 4, 6, 7}
};

const int Tetra::six_tetra_neighbors[6][4][4] = {
	{{1, 0, 0, 5}, {0, 0, 0, 4}, {0, 0, 0, 1}, {0, -1, 0, 2}},
	{{1, 0, 0, 3}, {0, 0, 0, 2}, {0, 0, 0, 0}, {0, 0, -1, 4}},
	{{0, 1, 0, 0}, {0, 0, 0, 1}, {0, 0, 0, 3}, {0, 0, -1, 5}},
	{{0, 1, 0, 4}, {0, 0, 0, 5}, {0, 0, 0, 2}, {-1, 0, 0, 1}},
	{{0, 0, 1, 1}, {0, 0, 0, 0}, {0, 0, 0, 5}, {0, -1, 0, 3}},
	{{0, 0, 1, 2}, {0, 0, 0, 3}, {0, 0, 0, 4}, {-1, 0, 0, 0}}
};

Tetra::Tetra(const float p1[3], const float p2[3], const float p3[3], const float p4[3], 
			 float _v1, float _v2, float _v3, float _v4)
{
	int i;
	for (i = 0; i < 3; i++)
	{
		x1[i] = p1[i];
		x2[i] = p2[i];
		x3[i] = p3[i];
		x4[i] = p4[i];
	}
	v1 = _v1;
	v2 = _v2; 
	v3 = _v3;
	v4 = _v4;
	// original vertex order
	for (i = 0; i < 4; i++)
	{
		vids[i] = i;
	}

	sortVerts();
	computeAreas();
}

double Tetra::isosurfAera(float _fx)
{
	double fx = _fx;
	double s;
	
	// zero isosurf area is fx is out of the range
	if(fx <= v1) return 0.0f;
	if(fx >= v4) return 0.0f;
	
	if(fx < v2) {
		s = (fx - v1) / (v2 - v1);
		return s*s*area1;
	} else if (fx >= v3) {
		s = (v4 - fx) / (v4 - v3);
		return s*s*area2;
	}
	
	// fx is between v2 and v3
	// isosurfe is a quad. We divide it to two triangles
	double area = 0;
	
	area += ((v3 - fx)/(v3 - v2)) * ((fx-v1)/(v2-v1)) * area1;
	area += ((v4 - fx)/(v4 - v3)) * ((fx-v2)/(v3-v2)) * area2;
	return (float) area;
}

double Tetra::innerVolume(float _fx)
{
	double s, s2, s3;
	double cum1 = 0;
	double val;

	double fx = _fx;
	// return 0 if fx <= v1
	if (fx <= v1) return 0.0f;

	if (fx >= v4) return volume;

	// compute the
	if (fx < v2)
	{
		if (v1 == v2)
			return 0;
		else
		{
			s = (fx-v1)/(v2-v1);
			return factor*s*s*s*area1*(v2-v1)/3.0;
		}
	}

	cum1 = area1*(v2-v1)/3.0;

	if (fx < v3)
	{
		s = (fx - v2)/(float)(v3-v2);
		s2 = s*s;
		s3 = s2*s;
		val = cum1 + (area1*(s - s2 + s3/3.0)
					  + 2*midarea*(s2/2.0 - s3/3.0)
					  + area2*(s3/3.0)) * (v3-v2);
		return factor*val;
	}

	cum1 += (area1/3.0 + midarea/3.0 + area2/3.0) * (v3-v2);

	if (fx < v4)
	{
		if (v4==v2)
			val = area2;
		else
		{
			s = (fx-v3)/(float)(v4-v3);
			s2 = s*s;
			s3 = s2*s;
			val = cum1 + (area2*(s - s2 + s3/3.0)) * (v4-v3);
#ifdef DEBUG_VOLVOL
			printf("val (%f) = %f\n", fx, (1.0-s)*(1.0-s) * area2);
#endif
		}
		return factor * val;
	}

	cum1 += area2/3.0 * (v4-v3);

	return factor * cum1;
}


double Tetra::funcIntegral(float _f1, float _f2, float _f3, float _f4, float _fx)
{
	double t;
	double f[4];
	double fx = _fx;

#ifdef _DEBUG
	assert(v1 < v2 && v2 < v3 && v3 < v4); 
#endif

	// reorder the function values
	f[0] = _f1; f[1] = _f2; f[2] = _f3; f[3] = _f4;
	double f1 = f[vids[0]]; 
	double f2 = f[vids[1]];
	double f3 = f[vids[2]];
	double f4 = f[vids[3]];

	if (fx <= v1) return 0;

    if (fx >= v4) return volume*(f1+f2+f3+f4)/4;

	double s, vol, funcIntegral, func[4];

	func[0] = f1;
	func[1] = f2;
	func[2] = (v2-v1)*f4/(v4-v1) + (v4-v2)*f1/(v4-v1);
	func[3] = (v2-v1)*f3/(v3-v1) + (v3-v2)*f1/(v3-v1);
	//printf("v: %f %f %f %f, f: %f %f %f %f\n", v1, v2, v3, v4, f1, f2, f3, f4);
	if (fx <= v2)
	{
		s = (fx - v1) / (v2 - v1);
		vol = factor*area1*(v2-v1)*s*s*s / 3;
		//printf("vol: %f, func: %f %f %f %f\n", vol, func[0], func[1], func[2], func[3]);
		return vol * ((4-3*s)*func[0] + s*func[1] + s*func[2] + s*func[3])/4;
	}
	double cum1 = factor*area1*(v2-v1)*(func[0]+func[1]+func[2]+func[3])/12;

	double A = f3-f2 + (v3-v2)*(f4-f1)/(v4-v1) + (v3-v2)*(f3-f1)/(v3-v1);
	double B = f2 + (v2-v1)*f4/(v4-v1) + (v4-v2)*f1/(v4-v1) + (v2-v1)*f3/(v3-v1) + (v3-v2)*f1/(v3-v1);
	double r = (v3-v2)/(v2-v1);
	double C = f3-f2 + (v3-v2)*(f4-f1)/(v4-v1) + (v3-v2)*(f4-f2)/(v4-v2);
	double D = f2 + (v2-v1)*f4/(v4-v1) + (v4-v2)*f1/(v4-v1) + f2;
	t = (v3-v2)/(v4-v3);        
	if (fx <= v3)
	{
		s = (fx-v2)/(v3-v2);
		double s2 = s*s;
		double s3 = s2*s;
		double s4 = s3*s;
		//float funcIntegral1 = factor*area1*(v3-v2)*(B*s+ (A+B*(r-1))*s2/2 + ((r-1)*A-r*B)*s3/3 - A*r*s4/4)/3;
		double funcIntegral1 = factor*(v3-v2)*s*(A*s*(area1*(6-8*s+3*s2)+(4-3*s)*s*midarea)+2*B*(2*area1*(3-3*s+s2)+(3-2*s)*s*midarea))/36;
		//float funcIntegral2 = factor*area2*(v3-v2)*(D*(1+t)*s2/2+(C*(1+t)-D*t)*s3/3-C*t*s4/4)/3;
		double funcIntegral2 = factor*(v3-v2)*s2*(C*s*(3*s*(area2-midarea)+4*midarea)+D*(4*s*(area2-midarea)+6*midarea))/36;
#ifdef DEBUG_VOLVOL
		printf("A = %f, B = %f, C = %f, D = %f, r = %f, t = %f, s = %f\n", A, B, C, D, r, t,s );
		printf("funcIntegral1 = %.10f, funcIntegral2 = %.10f, cum1 = %f\n", funcIntegral1, funcIntegral2, cum1+funcIntegral1+funcIntegral2);
#endif
		return cum1+funcIntegral1+funcIntegral2;
	}
	//cum1 += ((B/2 + B*r/6 + A/6 + A*r/12)*area1 + (D/2 + D*t/6 + C/3 + C*t/12)*area2)*factor*(v3-v2)/3;
	cum1 += factor*(v3-v2)*((A*(area1+midarea)+2*B*(2*area1+midarea))/36 + (2*D*(2*area2+midarea)+C*(3*area2+midarea))/36);

	// v3 < fx < v4
	func[0] = f4;
	func[1] = f3;
	func[2] = (v4-v3)*f1/(v4-v1) + (v3-v1)*f4/(v4-v1);
	func[3] = (v4-v3)*f2/(v4-v2) + (v3-v2)*f4/(v4-v2);
	s = (fx-v3)/(v4-v3);
	funcIntegral = factor*area2*(v4-v3)*((func[0]+func[1]+func[2]+func[3]) - 
								 (1-s)*(1-s)*(1-s)*((1+3*s)*func[0]+(1-s)*func[1]+(1-s)*func[2]+(1-s)*func[3]))/12;

#ifdef DEBUG_VOLVOL
	printf("fx = %f, cum1 = %f\n", fx, cum1+funcIntegral);
#endif
	return cum1+funcIntegral;   
}

void Tetra::multiFuncsIntegral(const float *fv1, const float *fv2, const float *fv3, 
							   const float *fv4, float *out, int n, float fx)
{
	int i;

	assert(v1 < v2 && v2 < v3 && v3 < v4);

	// reorder the functions 
	const float* pf[4];
	pf[0] = fv1; pf[1] = fv2; pf[2] = fv3; pf[3] = fv4;
	const float *f1, *f2, *f3, *f4;
	f1 = pf[vids[0]]; 
	f2 = pf[vids[1]];
	f3 = pf[vids[2]];
	f4 = pf[vids[3]];

	for (i = 0; i < n; i++)	out[i] = 0;

	// return 0 if fx <= v1
	if (fx <= v1) return;

	if (fx >= v4)
	{
		for (i = 0; i < n; i++)
		{
			out[i] = (float) (volume * (f1[i]+f2[i]+f3[i]+f4[i])/4);
		}
		return;
	}


	double s, vol, funcIntegral, func[4];
	double *cum1 = new double[n];

	if (fx <= v2)
	{
		s = (fx - v1) / (v2 - v1);
		vol = factor*area1*(v2-v1)*s*s*s / 3;

		for (i = 0; i < n; i++)
		{
			func[0] = f1[i];
			func[1] = f2[i];
			func[2] = (v2-v1)*f4[i]/(v4-v1) + (v4-v2)*f1[i]/(v4-v1);
			func[3] = (v2-v1)*f3[i]/(v3-v1) + (v3-v2)*f1[i]/(v3-v1);
			out[i] = (float) (vol * ((4-3*s)*func[0] + s*func[1] + s*func[2] + s*func[3])/4);
		}
		delete[] cum1;
		return;
	}
	for (i = 0; i < n; i++)
	{
		func[0] = f1[i];
		func[1] = f2[i];
		func[2] = (v2-v1)*f4[i]/(v4-v1) + (v4-v2)*f1[i]/(v4-v1);
		func[3] = (v2-v1)*f3[i]/(v3-v1) + (v3-v2)*f1[i]/(v3-v1);
		cum1[i] = factor*area1*(v2-v1)*(func[0]+func[1]+func[2]+func[3])/12;
	}

	double A, B, C, D, r;      
	if (fx <= v3)
	{
		s = (fx-v2)/(v3-v2);
		double s2 = s*s;
		double s3 = s2*s;
		double s4 = s3*s;

		for (i = 0; i < n; i++)
		{
			A = f3[i]-f2[i] + (v3-v2)*(f4[i]-f1[i])/(v4-v1) + (v3-v2)*(f3[i]-f1[i])/(v3-v1);
			B = f2[i] + (v2-v1)*f4[i]/(v4-v1) + (v4-v2)*f1[i]/(v4-v1) + (v2-v1)*f3[i]/(v3-v1) + (v3-v2)*f1[i]/(v3-v1);
			r = (v3-v2)/(v2-v1);
			C = f3[i]-f2[i] + (v3-v2)*(f4[i]-f1[i])/(v4-v1) + (v3-v2)*(f4[i]-f2[i])/(v4-v2);
			D = f2[i] + (v2-v1)*f4[i]/(v4-v1) + (v4-v2)*f1[i]/(v4-v1) + f2[i];

			double funcIntegral1 = factor*(v3-v2)*s*(A*s*(area1*(6-8*s+3*s2)+(4-3*s)*s*midarea)+2*B*(2*area1*(3-3*s+s2)+(3-2*s)*s*midarea))/36;
			double funcIntegral2 = factor*(v3-v2)*s2*(C*s*(3*s*(area2-midarea)+4*midarea)+D*(4*s*(area2-midarea)+6*midarea))/36;
#ifdef DEBUG_VOLVOL
			printf("A = %f, B = %f, C = %f, D = %f, r = %f, s = %f\n", A, B, C, D, r, s );
			printf("funcIntegral1 = %.10f, funcIntegral2 = %.10f, cum1 = %f\n", funcIntegral1, funcIntegral2, cum1[i]+funcIntegral1+funcIntegral2);
#endif
			out[i] = (float)(cum1[i]+funcIntegral1+funcIntegral2);
		}
		delete[] cum1;
		return;
	}
	for (i = 0; i < n; i++)
	{
		A = f3[i]-f2[i] + (v3-v2)*(f4[i]-f1[i])/(v4-v1) + (v3-v2)*(f3[i]-f1[i])/(v3-v1);
		B = f2[i] + (v2-v1)*f4[i]/(v4-v1) + (v4-v2)*f1[i]/(v4-v1) + (v2-v1)*f3[i]/(v3-v1) + (v3-v2)*f1[i]/(v3-v1);
		r = (v3-v2)/(v2-v1);
		C = f3[i]-f2[i] + (v3-v2)*(f4[i]-f1[i])/(v4-v1) + (v3-v2)*(f4[i]-f2[i])/(v4-v2);
		D = f2[i] + (v2-v1)*f4[i]/(v4-v1) + (v4-v2)*f1[i]/(v4-v1) + f2[i];
		cum1[i] += factor*(v3-v2)*((A*(area1+midarea)+2*B*(2*area1+midarea))/36 + (2*D*(2*area2+midarea)+C*(3*area2+midarea))/36);
	}

	// v3 < fx < v4
	s = (fx-v3)/(v4-v3);
	for (i = 0; i < n; i++)
	{
		func[0] = f4[i];
		func[1] = f3[i];
		func[2] = (v4-v3)*f1[i]/(v4-v1) + (v3-v1)*f4[i]/(v4-v1);
		func[3] = (v4-v3)*f2[i]/(v4-v2) + (v3-v2)*f4[i]/(v4-v2);
		funcIntegral = factor*area2*(v4-v3)*((func[0]+func[1]+func[2]+func[3]) - 
									 (1-s)*(1-s)*(1-s)*((1+3*s)*func[0]+(1-s)*func[1]+(1-s)*func[2]+(1-s)*func[3]))/12;
		out[i] = (float)(cum1[i] + funcIntegral);
#ifdef DEBUG_VOLVOL
		printf("fx = %f, cum1 = %f\n", fx, cum1[i]+funcIntegral);
#endif
	}
	delete[] cum1;
	return;
}

//////////////////////////////////////////////////////////////////////////
// Private Functions
//////////////////////////////////////////////////////////////////////////

void Tetra::sortVerts()
{
	int i;
	DoubleIntPair verts[4];
	double coords[4][3];

	// trival case
	if (v1 < v2 && v2 < v3 && v3 < v4) return;
	
	verts[0].id = 0; verts[0].val = v1;
	verts[1].id = 1; verts[1].val = v2;
	verts[2].id = 2; verts[2].val = v3;
	verts[3].id = 3; verts[3].val = v4; 
	for (i = 0; i < 3; i++)
	{
		coords[0][i] = x1[i];
		coords[1][i] = x2[i];
		coords[2][i] = x3[i];
		coords[3][i] = x4[i];
	}
	//sort the vertices by their values
	qsort(verts, 4, sizeof(DoubleIntPair), DoubleIntPair::compare);
	double epsilon = std::max<double>(1e-6f, (verts[3].val - verts[1].val)/400000.0f);
	if (verts[1].val < verts[0].val+ epsilon) verts[1].val = verts[0].val + epsilon;
	if (verts[2].val < verts[1].val+ epsilon) verts[2].val = verts[1].val + epsilon;
	if (verts[3].val < verts[2].val+ epsilon) verts[3].val = verts[2].val + epsilon;

	// rearrange the vertices
	v1 = verts[0].val;
	v2 = verts[1].val;
	v3 = verts[2].val;
	v4 = verts[3].val;

	for (i = 0; i < 3; i++)
	{
		x1[i] = coords[verts[0].id][i];
		x2[i] = coords[verts[1].id][i];
		x3[i] = coords[verts[2].id][i];
		x4[i] = coords[verts[3].id][i];
	}

	for (i = 0; i < 4; i++)
	{
		vids[i] = verts[i].id;
	}
}

void Tetra::computeAreas()
{

	double mid[3], mid2[3];
	double vec1[3], vec2[3], vec3[3];
	double ival;

	double cp[3];

	//vector vec1 = x2 - x1
	vec1[0] = x2[0]-x1[0];
	vec1[1] = x2[1]-x1[1];
	vec1[2] = x2[2]-x1[2];
	//vector vec2 = x3-x1
	vec2[0] = x3[0]-x1[0];
	vec2[1] = x3[1]-x1[1];
	vec2[2] = x3[2]-x1[2];
	//vector vec3 = x4-x2
	vec3[0] = x4[0]-x1[0];
	vec3[1] = x4[1]-x1[1];
	vec3[2] = x4[2]-x1[2];
	// compute the total volume of the tetrahedron
	cp[0] = vec3[0]*(vec1[1]*vec2[2]-vec1[2]*vec2[1]);
	cp[1] = vec3[1]*(vec1[2]*vec2[0]-vec1[0]*vec2[2]);
	cp[2] = vec3[2]*(vec1[0]*vec2[1]-vec1[1]*vec2[0]);
    volume = fabs(cp[0] + cp[1] + cp[2])/6.0;

	// compute the first area
	if (v1 != v3)
		ival = (v3-v2)/(v3-v1);
	else
		ival = 0.0;
	// mid: point on edge x1x3 with value v2
	mid[0] = (1.0-ival)*x3[0] + (ival)*x1[0];
	mid[1] = (1.0-ival)*x3[1] + (ival)*x1[1];
	mid[2] = (1.0-ival)*x3[2] + (ival)*x1[2];

	if (v1 != v4)
		ival = (v4-v2)/(v4-v1);
	else
		ival = 0.0;
	// mid2: point on edge x1x4 with value v2
	mid2[0] = (1.0-ival)*x4[0] + (ival)*x1[0];
	mid2[1] = (1.0-ival)*x4[1] + (ival)*x1[1];
	mid2[2] = (1.0-ival)*x4[2] + (ival)*x1[2];

	// two vector of triangle x2, mid, mid2
	vec1[0] = mid[0]-x2[0];
	vec1[1] = mid[1]-x2[1];
	vec1[2] = mid[2]-x2[2];
	vec2[0] = mid2[0]-x2[0];
	vec2[1] = mid2[1]-x2[1];
	vec2[2] = mid2[2]-x2[2];
	// cross product
	cp[0] = vec1[1]*vec2[2]-vec1[2]*vec2[1];
	cp[1] = vec1[2]*vec2[0]-vec1[0]*vec2[2];
	cp[2] = vec1[0]*vec2[1]-vec1[1]*vec2[0];
	area1 = 0.5 * fabs(sqrt(cp[0]*cp[0] + cp[1]*cp[1] + cp[2]*cp[2]));

	// compute the second area similarly
	if (v2 != v4)
		ival = (v4-v3)/(v4-v2);
	else
		ival = 0.0;
	mid[0] = (1.0-ival)*x4[0] + (ival)*x2[0];
	mid[1] = (1.0-ival)*x4[1] + (ival)*x2[1];
	mid[2] = (1.0-ival)*x4[2] + (ival)*x2[2];
	if (v4 != v1)
		ival = (v4-v3)/(v4-v1);
	else
		ival = 0.0;
	mid2[0] = (1.0-ival)*x4[0] + (ival)*x1[0];
	mid2[1] = (1.0-ival)*x4[1] + (ival)*x1[1];
	mid2[2] = (1.0-ival)*x4[2] + (ival)*x1[2];

	vec1[0] = mid[0]-x3[0];
	vec1[1] = mid[1]-x3[1];
	vec1[2] = mid[2]-x3[2];
	vec2[0] = mid2[0]-x3[0];
	vec2[1] = mid2[1]-x3[1];
	vec2[2] = mid2[2]-x3[2];
	cp[0] = vec1[1]*vec2[2]-vec1[2]*vec2[1];
	cp[1] = vec1[2]*vec2[0]-vec1[0]*vec2[2];
	cp[2] = vec1[0]*vec2[1]-vec1[1]*vec2[0];
	area2 = 0.5 * fabs(sqrt(cp[0]*cp[0] + cp[1]*cp[1] + cp[2]*cp[2]));

	// what is midarea 
	if (v2-v1 >= v4-v3)
		midarea = area1 * (1.0+(v3-v2)/(v2-v1));
	else 
		midarea = area2 * (1.0+(v3-v2)/(v4-v3));

	double vol2 = ((v3-v1)*area1 + (v3-v2)*midarea + (v4-v2)*area2)/3;
	factor = volume / vol2;

#ifdef DEBUG_VOLVOL
	printf("1: %f 2: %f mid: %f\n", area1, area2, midarea);
	printf("volume = %f, vol2 = %f, factor = %f\n", volume, vol2, factor);
	printf("v1: %f v2: %f v3: %f v4: %f\n", v1, v2, v3, v4);
#endif
}

void Tetra::extractSurface(int id[4], float isoval, GeometryData *surface, EdgeCache &cache)
{
	// reorder the vertex index  
	int vertId[4];
	int i;
	for(i = 0; i < 4; i++) {
		vertId[i] = id[vids[i]];
	}
	
	assert(v1 < v2 && v2 < v3 && v3 < v4); 
	// no intersection if isoval <= v1 or isoval >= v4
	if(isoval <= v1 || isoval >= v4) return;

	Vector3 vec, normal;
	vec[0] = (float) (x4[0] - x1[0]);
	vec[1] = (float) (x4[1] - x1[1]);
	vec[2] = (float) (x4[2] - x1[2]);

	// if isoval <= v2, the intersection is a triangle
	if(isoval <= v2) {
		int p1 = interpEdge(vertId[0], vertId[1], x1, x2, v1, v2, isoval, surface, cache); 
		int p2 = interpEdge(vertId[0], vertId[2], x1, x3, v1, v3, isoval, surface, cache);
		int p3 = interpEdge(vertId[0], vertId[3], x1, x4, v1, v4, isoval, surface, cache);
		normal = faceNormal(surface, p1, p2, p3);
		if(vec.dotProduct(normal) >= 0) {
			surface->addTriangle(p1, p2, p3);
		} else {
			surface->addTriangle(p1, p3, p2);
		}
		return;
	}

	// if isoval >= v3, the intersection is also a triangle
	if(isoval >= v3) {
		int p1 = interpEdge(vertId[0], vertId[3], x1, x4, v1, v4, isoval, surface, cache);
		int p2 = interpEdge(vertId[1], vertId[3], x2, x4, v2, v4, isoval, surface, cache);
		int p3 = interpEdge(vertId[2], vertId[3], x3, x4, v3, v4, isoval, surface, cache);
		normal = faceNormal(surface, p1, p2, p3);
		if(vec.dotProduct(normal) >= 0) {
			surface->addTriangle(p1, p2, p3);
		} else {
			surface->addTriangle(p1, p3, p2);
		}
		return;
	}

	// here v2 < isoval < v3, the intersection is a quad
	int p1 = interpEdge(vertId[0], vertId[2], x1, x3, v1, v3, isoval, surface, cache);
	int p2 = interpEdge(vertId[1], vertId[2], x2, x3, v2, v3, isoval, surface, cache);
	int p3 = interpEdge(vertId[1], vertId[3], x2, x4, v2, v4, isoval, surface, cache);
	int p4 = interpEdge(vertId[0], vertId[3], x1, x4, v1, v4, isoval, surface, cache);
	normal = faceNormal(surface, p1, p2, p4);
	if(vec.dotProduct(normal) >= 0) {
		surface->addTriangle(p1, p2, p4);
		surface->addTriangle(p2, p3, p4);
	} else {
		surface->addTriangle(p1, p4, p2);
		surface->addTriangle(p2, p4, p3);
	}
	return;
}

int Tetra::interpEdge(int vid1, int vid2, double coord1[3], double coord2[3], double val1, double val2,
					  float isoval, GeometryData* surface, EdgeCache& cache)
{
	EdgeEndPoints edge(vid1, vid2);

	int vert = cache.find(edge);
	// found in the cache
	if(vert >= 0) {
		return vert;
	}
	// otherwise, compute the the new intersection point
	vert = cache.insert(edge);
	// vector from p1 to p2
	double vec[3];
	int i;
	for(i = 0; i < 3; i++) {
		vec[i] = coord2[i]-coord1[i];
	}
	double interp = (isoval - val1) / (val2 - val1);
	Vector3 pts, normal;
	for(i = 0; i < 3; i++) {
		pts[i] = (float) (coord1[i] + interp*vec[i]);
		normal[i] = (float) vec[i];
	}
	// normalize3f(normal);
	// surface->addVertex(pts, normal);
	surface->addVertex(pts);	// cannot compute normal correctly in a tetrahedron

	return vert;
}
